/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2022 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "system.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

#define XUNJI_MODE    0xD1
#define LIDAR_MODE    0xD2
#define PS2_MODE      0xD3
#define BLINKER_MODE  0xD4
#define BLUE_MODE     0xD5

uint8_t ROBOT_MODE = LIDAR_MODE;

int fputc(int ch,FILE *f)
{
    uint32_t temp = ch;
 
    HAL_UART_Transmit(&huart1,(uint8_t *)&temp,1,0xFFFF);        //huart1�Ǵ��ڵľ��
    HAL_Delay(2);
 
    return ch;
}
void HEXAPOD_Data_Processing(void);
void Printf_Data(void);
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
uint16_t PPM_Sample_Cnt = 0;//ͨ��
uint8_t PPM_Chn_Max = 8;//���ͨ����
uint32_t PPM_Time = 0;//��ȡͨ��ʱ��
uint16_t PPM_Okay = 0;//��һ�ν���״̬
uint16_t PPM_Databuf[8] = {0};//����ͨ��������
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for myTask02_led */
osThreadId_t myTask02_ledHandle;
const osThreadAttr_t myTask02_led_attributes = {
  .name = "myTask02_led",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for myTask03_print */
osThreadId_t myTask03_printHandle;
const osThreadAttr_t myTask03_print_attributes = {
  .name = "myTask03_print",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for myTask04 */
osThreadId_t myTask04Handle;
const osThreadAttr_t myTask04_attributes = {
  .name = "myTask04",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);
void StartTask02(void *argument);
void StartTask03(void *argument);
void StartTask04(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* creation of myTask02_led */
  myTask02_ledHandle = osThreadNew(StartTask02, NULL, &myTask02_led_attributes);

  /* creation of myTask03_print */
  myTask03_printHandle = osThreadNew(StartTask03, NULL, &myTask03_print_attributes);

  /* creation of myTask04 */
  myTask04Handle = osThreadNew(StartTask04, NULL, &myTask04_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
  for(;;)
  {
    switch (ROBOT_MODE)
    {
    case XUNJI_MODE:
      /* code */
			xunji();
      break;
    case LIDAR_MODE:
      /* code */
		  Lidar_Avoid();
			X_Speed = map(Move_X, -100, 100, -30, 30);
			Y_Speed = map(0, 			-100, 100, -30, 30);
			Z_Speed = map(Move_Z, 1, 		-1,	 -60, 60);
      break;
    case PS2_MODE:
      /* code */
      break;
    case BLINKER_MODE:
      /* code */
      break;
    case BLUE_MODE:
      /* code */
      break;

    default:
      break;
    }
		inverse_kinematics(X_Speed, Y_Speed, Z_Speed, Current_Yaw, Wheel_target_speed);	
		Run_All_Motor(Wheel_target_speed);
		if(Start_flag == 1)
		{
			#ifdef enableMPU6050
			if(First_Get_Yaw_flag == 0)			//��ȡ�ȶ����yaw�ĳ�ʼֵ
			{
				Original_Yaw = Yaw;
				First_Get_Yaw_flag = 1;				//ֻ��ȡһ��
			}
			#endif
		}
		else
		{
			osDelay(10);	
		}
		
		#ifdef enableMPU6050
		if(Start_flag == 0)
		{
			Time_left++;
			if(Time_left > 20)
			{
				Start_flag = 1;
			}
			printf("Time_left:   %d\n", 22-Time_left);
			osDelay(1000);	
		#endif
  osDelay(10);	
  }
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_StartTask02 */
/**
* @brief Function implementing the myTask02_led thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask02 */
void StartTask02(void *argument)
{
  /* USER CODE BEGIN StartTask02 */
  /* Infinite loop */
  for(;;)
  {
		#ifdef enableMPU6050
		Read_DMP();
		Current_Yaw = Original_Yaw - Yaw;	
		#endif
		HAL_ADC_Start_DMA(&hadc1,(uint32_t*)uhADCxConvertedValue,1);
		VoltageValue[0] = uhADCxConvertedValue[0]*3.3f/4095;

		SHT30_read_result();
		environment_temp = Th30BufTypedef.TH30_temp * 100;
		environment_hum  = Th30BufTypedef.TH30_hum	* 100;
		osDelay(50);		
  }
  /* USER CODE END StartTask02 */
}

/* USER CODE BEGIN Header_StartTask03 */
/**
* @brief Function implementing the myTask03_print thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask03 */
void StartTask03(void *argument)
{
  /* USER CODE BEGIN StartTask03 */
  /* Infinite loop */
  for(;;)
  {
		#ifdef enableOLED
		OLED_ShowString(0,0,"Bat",16); OLED_ShowNum(40,0,(int)(VoltageValue[1]*6.04f),8,16);
		OLED_ShowString(0,2,"Start",16); OLED_ShowNum(40,2,Start_flag,8,16);
	  if(Yaw < 0)
	  {
			OLED_ShowString(0,4,"Yaw--",16); OLED_ShowNum(40,4,(int)-Yaw,8,16);
	  }
	  else
	  {
			OLED_ShowString(0,4,"Yaw  ",16); OLED_ShowNum(40,4,(int)Yaw,8,16);
	  }
		OLED_ShowNum(0,4,Original_Yaw,8,16);
		OLED_ShowNum(0,6,Time_left,8,16);
		OLED_ShowNum(0,4,X_Speed,8,16);
		#endif
//		PS2_Data_Processing();
//		Printf_Data();	
		usart2_data_transition();
    osDelay(100);
  }
  /* USER CODE END StartTask03 */
}

/* USER CODE BEGIN Header_StartTask04 */
/**
* @brief Function implementing the myTask04 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask04 */
void StartTask04(void *argument)
{
  /* USER CODE BEGIN StartTask04 */
  /* Infinite loop */
  for(;;)
  {
		if((VoltageValue[0]*6.00f > 16) || (Start_flag == 1))
		{
			Battery_Ready = 1;
			PCout(13) = !PCout(13);
			osDelay(1000);
		}
		else
		{
			Battery_Ready = 0;
			PCout(13) = !PCout(13);
			//PAout(5) = !PAout(5);
			osDelay(50);
		}
	}
  /* USER CODE END StartTask04 */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
/* USER CODE BEGIN 4 */
//PPM�����жϻص�����
#ifdef enablePPM
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    if(GPIO_Pin==GPIO_PIN_10)//�ж��Ƿ�Ϊ�������������жϣ���������ΪPIN8
    {
        PPM_Time = TIM4 ->CNT;//����ʱ��ת��
        TIM4 -> CNT = 0;//����������
        if (PPM_Okay == 1)//�ж��Ƿ����µ�һ�ֽ���
        {
            PPM_Sample_Cnt++;//ͨ����+1
            PPM_Databuf[PPM_Sample_Cnt - 1] = PPM_Time;//��ÿһ��ͨ������ֵ��������
            if (PPM_Sample_Cnt >= PPM_Chn_Max)//�ж��Ƿ񳬹��ͨ����
                PPM_Okay = 0;
        }
        if (PPM_Time >= 2050)//��ʱ�����½��ؼ���ͨ�����ݣ�������һ�ֽ���
        {
            PPM_Okay = 1;
            PPM_Sample_Cnt = 0;
        }
    }
			if(fabs(PPM_Databuf[7] - 650) < 20)		//����RCң�ػ���PS2ң��
			{
				RC_flag = 1;
			}	
			if(fabs(PPM_Databuf[7] - 1010) < 20)
			{
				RC_flag = 0;
			}
			RC_LY = map(PPM_Databuf[2],650,1370,0,255);
			RC_LX = map(PPM_Databuf[3],650,1370,0,255);
			RC_RX = map(PPM_Databuf[0],650,1370,0,255);
			RC_RY = map(PPM_Databuf[1],650,1370,0,255);
			X_Speed = map(RC_LX,0,255,-100,100);
			Y_Speed = map(RC_LY,0,255,-100,100);
			Z_Speed = map(RC_RX,0,255,-100,100);
}

//CH1:RX 650-1370
//CH2:RY
//CH3:LY
//CH4:LX

//CH5:LK 650-1370
//CH6:RK
//CH7:LB 650-1010-1370
//CH8:RB
#endif

void Printf_Data(void)
{
//	printf("PS2_LX:%d ",PS2_LX);
//	printf("PS2_LY:%d ",PS2_LY);
//	printf("PS2_RX:%d ",PS2_RX);
//	printf("PS2_RY:%d ",PS2_RY);
//	printf("PS2_KEY:%d \r\n",PS2_KEY);
//	printf("current X: %f",HOME_X[0]);
//	printf("current Y: %f",HOME_Y[0]);
//	printf("current Z: %f",HOME_Z[0]);
//	printf("current Z: %f",TIBIA_LENGTH);
//	printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",(float)Servo[6],(float)Servo[15],(float)Servo[0],(float)Servo[9],(float)Servo[3],(float)Servo[12]);
//			for (uint8_t i = 0; i < PPM_Chn_Max; i++)
//        printf("chn%d:%d ", i+1, PPM_Databuf[i]);
//		printf("temp:%.2f,hum:%.2f,voltage:%.2f",Th30BufTypedef.TH30_temp,Th30BufTypedef.TH30_hum,(double)VoltageValue[0]);
}

/* USER CODE END 4 */
/* USER CODE END Application */

